/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.image.RGBImage;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Blayne
 *
 *  ***  This code probably does not work yet! Needs testing once camera is attached ***
 */
public class CameraSystem extends Subsystem
{
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    private AxisCamera camera;
    private CriteriaCollection cc;

    double distance;
    double angle;

    double TopLeft_X;
    double TopLeft_Y;
    double TopRight_X;
    double TopRight_Y;
    double BottomLeft_X;
    double BottomLeft_Y;
    double BottomRight_X;
    double BottomRight_Y;

    BinaryImage thresholdImage;
    BinaryImage bigObjectsImage;
    BinaryImage convexHullImage;
    BinaryImage filteredImage;

    ParticleAnalysisReport[] reports;

    private static CameraSystem instance = null;

    public static CameraSystem getInstance() {
        if (instance == null)
            instance = new CameraSystem();
        return instance;
    }

    public void CameraSystem()
    {
        camera=AxisCamera.getInstance();

        distance=0.0;
        angle=0.0;
        TopLeft_X=0.0;
        TopLeft_Y=0.0;
        TopRight_X=0.0;
        TopRight_Y=0.0;
        BottomLeft_X=0.0;
        BottomLeft_Y=0.0;
        BottomRight_X=0.0;
        BottomRight_Y=0.0;

        reports = null;

        thresholdImage = null;
        bigObjectsImage = null;
        convexHullImage = null;
        filteredImage = null;

        cc = new CriteriaCollection();      // create the criteria for the particle filter
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());

        //ADD COMMAND HERE setDefaultCommand(new ());
    }

    public void getFilteredImageData()
    {        
        try{
            ColorImage image;
            
            image = new RGBImage("/1a.JPG");
            thresholdImage = image.thresholdRGB(0, 255, 200, 255, 0, 255);   // keep only highly green objects
            bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles
            filteredImage.write("/out.jpg");
            
            reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of results

            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                System.out.println("Particle: " + i + ":  Center of mass x: " + r.center_mass_x + ", y: " + r.center_mass_y);
            }
            System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());

            //Free image memory
            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();

            thresholdImage = null;
            bigObjectsImage = null;
            convexHullImage = null;
            filteredImage = null;

            image.free();
            image = null;
        }
        catch (Exception ex) {
            ex.printStackTrace();
        }
    }

    public void CalculateTargetDistance()
    {
        //Use the width(?) of the target rectangle to determine distance
    }

    public void CalculateTargetAngle()
    {
        //use the skew of the height on the left and right sides to determine viewing angle of the target
    }

}